#!/usr/bin/env python
"""Node steps through a series of tasks to perform.

Tasks (or waypoints) are loaded from parameters, and then each one is used to
calculate sailing_state and goal_heading until its check_end_condition()
returns True.
"""

import rospy
from std_msgs.msg import Float32
from std_msgs.msg import Float64
from std_msgs.msg import String
from dynamic_reconfigure.server import Server
import sailing_robot
from sailing_robot.tasks import tasks_from_wps
from sailing_robot.tasks_ros import RosTasksRunner
from sailing_robot.navigation import Navigation
from sailing_robot.cfg import TackVotingConfig
from sensor_msgs.msg import NavSatFix


def goal_heading_publisher(tasks_runner):
    pub = rospy.Publisher("goal_heading", Float32, queue_size=10)
    pub_state = rospy.Publisher("sailing_state", String, queue_size=10)
    rate = rospy.Rate(rospy.get_param("config/rate"))

    tasks_runner.start_next_task()
    while not rospy.is_shutdown():
        state, goal_heading = tasks_runner.calculate_state_and_goal()
        pub.publish(goal_heading)
        pub_state.publish(state)
        rate.sleep()

def jibe_tack_now(msg):
    tasks_runner.insert_task({
        'kind': 'jibe_tack_now',
        'action': msg.data,
    })


def insert_waypoint(msg):
    tasks_runner.insert_task({
        'kind': 'to_waypoint',
        'waypoint_ll': (msg.latitude, msg.longitude),
        'target_radius': 2.5,     # set default value, hot fix for the force jibing node
        'tack_voting_radius': 1 # set default value, hot fix for the force jibing node
    })

def tack_voting_callback(config, level):
    """
    get updates for the dynamic parameters
    """
    radius = config.radius
    samples = config.samples
    threshold = config.threshold
    return config


if __name__ == '__main__':
    try:
        rospy.init_node("publish_goal_heading", anonymous=True)
        #tasks = rospy.get_param("tasks")
        tasks = tasks_from_wps(rospy.get_param("wp"))
        nav_options = rospy.get_param("navigation")
        nav = Navigation(**nav_options)
        tasks_runner = RosTasksRunner(tasks, nav)

        rospy.Subscriber('heading', Float32, nav.update_heading)
        rospy.Subscriber('wind_direction_apparent', Float64, nav.update_wind_direction)
        rospy.Subscriber('position', NavSatFix, nav.update_position)
        rospy.Subscriber('temporary_wp', NavSatFix, insert_waypoint)
        rospy.Subscriber('jibe_tack_now', String, jibe_tack_now)
        srv = Server(TackVotingConfig, tack_voting_callback)

        goal_heading_publisher(tasks_runner)
    except rospy.ROSInterruptException:
        pass
